/*
 * line_following.cpp
 *
 *  Created on: May 28, 2011
 *      Author: minyaung
 */

#include <line_following/line_following.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

line_following::line_following() :
	transport(lfNodeHandle), kSize(150), queueSizes(50), sensorHeight(0.25),
		xPosition(0), yPosition(0), pixelToInch(20), lowThreshold(470),
			facingAngle(0), seekerName("A")
{
	color_sub = transport.subscribe("color_in", 1, &line_following::convolute,
			this);
	nav_subscriber = lfNodeHandle.subscribe("seeker_pose", 10,
			&line_following::poseCallback, this);
	lf_publisher = lfNodeHandle.advertise<geometry_msgs::Point> (
			"breaks_locator", queueSizes);
	line_publisher = lfNodeHandle.advertise<geometry_msgs::Point> (
			"line_params", queueSizes);

#ifdef DEBUG
	cv::namedWindow(WINDOW1);
	cv::namedWindow(WINDOW2);
#endif
}

line_following::~line_following() {
#ifdef DEBUG
	cv::destroyWindow(WINDOW1);
	cv::destroyWindow(WINDOW2);
#endif
}

void line_following::poseCallback(const geometry_msgs::PoseStampedConstPtr& msg) {
	tf::Stamped<tf::Pose> pose;
	tf::poseStampedMsgToTF(*msg, pose);

	xPosition = pose.getOrigin().x();
	yPosition = pose.getOrigin().y();
	//std::cout << "seeker_pose: xPosition: " << xPosition << ", yPosition: " << yPosition << std::endl;
	facingAngle = pose.getRotation().getAngle();
}

void line_following::convolute(const sensor_msgs::ImageConstPtr& msg) {
	cv_bridge::CvImageConstPtr cv_ptr;
	try {
		cv_ptr = cv_bridge::toCvShare(msg, enc::BGR8);
	} catch (cv_bridge::Exception& e) {
		ROS_ERROR("cv_bridge exception: %s", e.what());
		return;
	}

	// Divides multi-channel array into several single-channel arrays
	cv::Mat colorChannels[3];
	cv::split(cv_ptr->image, colorChannels);

 // cv::Mat avgImg(colorChannels[0].rows, colorChannels[0].cols, CV_16UC1,
//cv::Scalar(0));
  cv::Mat blueMask = colorChannels[0] > colorChannels[1];
	cv::Mat blueMask2 = colorChannels[0] > colorChannels[2];
	bitwise_and(blueMask, blueMask2, blueMask);

	cv::Mat blueEdges;
	cv::Sobel(blueMask, blueEdges, CV_8U, 1, 0);


	std::vector<cv::Vec4i> lines;
	cv::HoughLinesP(blueEdges, lines, 1, CV_PI/180.0, 100, 50, 10);
#ifdef DEBUG
	cv::Mat lineImage = cv::Mat::zeros(blueMask.rows, blueMask.cols, CV_8U);
#endif
	float slope = 0;
	float offset = 0;
	int numLines = 0;
	for(unsigned int i=0; i<lines.size(); ++i)
	{
#ifdef DEBUG
		cv::line( lineImage, cv::Point(lines[i][0], lines[i][1]), cv::Point(lines[i][2],
			lines[i][3]), cv::Scalar(255));
		std::cout << "ONE: " << lines[i][0] << ", " << lines[i][1] << " TWO: " <<
lines[i][2] << ", " << lines[i][3] << std::endl;
#endif
		int liney1 = lines[i][1];
		int linex1 = lines[i][0];
		int liney2 = lines[i][3];
		int linex2 = lines[i][2];
		

		int lowy = liney2;
		int lowx = linex2;
		int highx = linex1;
		int highy = liney1;
		if(liney1 < liney2)
		{
			highy = liney2;
			highx = linex2;
			lowx = linex1;
			lowy = liney1;
		}

		if(lowy > lowThreshold)
		{
			float tempSlope = (float)(highx - lowx)/(float)(highy - lowy);
			slope += tempSlope; 
			offset += highx - tempSlope*highy;
			++numLines;
		}
	}

#ifdef DEBUG
	cv::imshow("Blue Edges", blueEdges);
	cv::imshow("Lines", lineImage);
	cv::waitKey(3);
#endif


	if(numLines != 0)
	{
		offset /= numLines;
		slope /= numLines;
		geometry_msgs::Point lpPoint;

		int& rows = blueEdges.rows;
		int& cols = blueEdges.cols;
		lpPoint.x = offset + slope*rows - (cols/2); // Intercept relative to bottom-center of image
		lpPoint.y = -slope; // Slope when +x goes from bottom to top

		line_publisher.publish(lpPoint);

		cv::Mat yBlue(1, rows, CV_8U);
		cv::Mat yGreen(1, rows, CV_8U);
		cv::Mat yRed(1, rows, CV_8U);
		for (int x = 0; x < rows; ++x) {
			unsigned int index = offset + slope*x;
			if(index < colorChannels[0].cols)
			{
				yBlue.at<char> (0, x) = colorChannels[0].at<char> (x, index);
				yGreen.at<char> (0, x) = colorChannels[1].at<char> (x, index);
				yRed.at<char> (0, x) = colorChannels[2].at<char> (x, index);
			}
			else
			{
				yBlue.at<char>(0,x) = 0;
				yGreen.at<char>(0,x) = 0;
				yRed.at<char>(0,x) = 0;
			}
		}
	
		int pixelOffset = 15;
		unsigned int blue, green, red;
		std::vector<int> whitespots;
	
		// Create a whitespots array with the pixel value at that point.
		for (int l = 0; l < yBlue.cols; l++) {
			blue = yBlue.at<unsigned char> (0, l);
			green = yGreen.at<unsigned char> (0, l);
			red = yRed.at<unsigned char> (0, l);
			if (red > (blue - pixelOffset) && red < (blue + pixelOffset) &&
			    red > (green - pixelOffset) && red < (green + pixelOffset)) {
				//std::cout << "red: " << red << ", blue: " << blue << ", green: " << green << ", l: " << l << std::endl;
				whitespots.push_back(l);
			}
		}
	
		int prev = 0;
		// Stores a list of initial indexes of the breaks in the tape line
		std::vector<int> breakIndices;
		int firstBreakIndex = 0;
		int counter = 0;
		int pixelsBreakStretch = 20;
	
		if (whitespots.size() > 0) {
			prev = whitespots[0];
			firstBreakIndex = prev;
		}
	
		for (unsigned int n = 1; n < whitespots.size(); n++) {
			counter++;
			if (whitespots[n] != prev + 1) {
				firstBreakIndex = whitespots[n];
				counter = 0;
			}
	
			prev = whitespots[n];
	
			if (counter == pixelsBreakStretch) {
				breakIndices.push_back(firstBreakIndex);
			}
		}

		unsigned int numPoints = breakIndices.size();
	
		geometry_msgs::Point point;
	
		double distance = 0.0;
		double xBreakPos = 0.0;
		double yBreakPos = 0.0;
	
		//generate data for each break point
		for (unsigned int i = 0; i < numPoints; ++i)
		{
			// Convert pixels to coordinates
			// First: Convert pixels to distance
			distance = computeBreakDistance(breakIndices[i], rows);
			printf( "breakPosition: %i, distance: %4.4f \n", breakIndices[i], distance );
	
			// Second: Find X and Y of the current location and angle facing
	
			if ( (facingAngle >= 0 && facingAngle <= PI/4) ||
					(facingAngle >= (7*PI)/4 && facingAngle <= 2*PI) )
			{
				printf("First quarter: ");
				xBreakPos = xPosition + distance;
				yBreakPos = round(yPosition);
			}
			else if (facingAngle <= (3*PI)/4 && facingAngle > PI/4)
			{
				printf("Second quarter: ");
				xBreakPos = round(xPosition);
				yBreakPos = yPosition + distance;
			}
			else if ( facingAngle <= (5*PI)/4 && facingAngle > (3*PI)/4 )
			{
				printf("Third quarter: ");
				xBreakPos = xPosition - distance;
				yBreakPos = round(yPosition);
			}
			else if (facingAngle <= (7*PI)/4 && facingAngle > (5*PI)/4)
			{
				printf("Fourth quarter: ");
				xBreakPos = round(xPosition);
				yBreakPos = yPosition - distance;
			}
	
			point.x = xBreakPos;
			point.y = yBreakPos;
	
			printf("xPosition: %i yPosition: %i \n", xPosition, yPosition );
			printf("Point X: %4.4f Y: %4.4f \n", point.x, point.y );
	
			lf_publisher.publish(point);
		}
	}	
}

double line_following::computeBreakDistance(int index, int rows) {
	double distance = 0.0;
	double tiltAngle = 0.0;
	double cameraFieldOfView = PI/3; // twice the inverse tangent of total

	//distanceToBreak = cameraHeightFromFloor*tan((PI/2 + tiltAngle) +
	//((double)index/image.rows - 0.5)*cameraFieldOfView);
    // sensorHeight = 6 inches ( 0.25 )
	distance = sensorHeight * tan((PI / 2 + tiltAngle) - ((double) index / rows - 0.5) * cameraFieldOfView);

	// = distance / pixelToInch;

	return distance;
}

void line_following::print(cv::Mat& mat, std::string name) {
	int cols = mat.cols, rows = mat.rows;
	std::cout << "Mat Name: " << name << ", rows: " << rows << ", cols: "
			<< cols << std::endl;

	for (int i = 0; i < rows; i++) {
		std::cout << "Rows : " << i << " Cols : " << cols
				<< "------------------" << std::endl;
		const float* Mi = mat.ptr<float> (i);
		for (int j = 0; j < cols; j++) {
			std::cout << Mi[j] << " ";
		}
		std::cout << std::endl;
	}
	std::cout << std::endl;
}

void line_following::print(std::vector<int>& vec, std::string name) {
	int size = vec.size();
	std::cout << "Vector Name: " << name << ", size: " << size << std::endl;
	for (int n = 0; n < size; n++) {
		std::cout << vec[n] << " ";
	}
	std::cout << std::endl;
}

void line_following::print(std::vector<char>& vec, std::string name) {
	int size = vec.size();
	std::cout << "Vector Name: " << name << ", size: " << size << std::endl;
	for (int n = 0; n < size; n++) {
		std::cout << (int) vec[n] << " ";
	}
	std::cout << std::endl;
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "line_following");
	line_following lf;
	ros::spin();
	return 0;
}

